/**
 * @file PIDController.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief PID控制器类
 */

#include "PIDController.h"
#include <cmath>
#include "utils.h"

/**
 * @brief 变量限幅
 * @param value 限幅变量
 * @param min 允许的最小值
 * @param max 允许的最大值
 */
void range(double &value, double min, double max) {
    if(value < min) {
        value = min;
    } else if(value > max) {
        value = max;
    }
}

/**
 * @brief 变量限幅
 * @param value 限幅变量
 * @param max 允许的最大值
 */
void range(double &value, double max) {
    max = std::abs(max);
    range(value, -max, max);
}

/**
 * @brief 设置PID参数
 * @param param PID参数
 */
void PIDController::setParam(PIDParam_t param) {
    this->param = param;
}

/**
 * @brief 设置控制精度
 * @param precision 新的控制精度
 */
void PIDController::setPrecision(double precision) {
    this->precision = precision;
}

/**
 * @brief 计算误差值
 * @param target 控制量目标值
 * @param current 控制量当前值
 * @return double 误差值
 */
double PIDController::calculateBias(double target, double current) {
    this->bias = current - target;
    return bias;
}

/**
 * @brief 获取误差值
 * @return double 误差值
 */
double PIDController::getBias() {
    return this->bias;
}

/**
 * @brief 计算PID输出值
 * @param target 控制量目标值
 * @param current 控制量当前值
 * @return double PID输出值
 */
double PIDController::calculateOutput(double target, double current) {
    calculateBias(target, current);

    if(!this->enable) {
        this->lastBias = 0;
        this->integralBias = 0;
        return 0;
    }

    double output = this->param.kp * bias + this->param.kd * (bias - this->lastBias);

    if(this->param.ki) {
        if(!this->param.intergralRadius || std::abs(this->bias) < this->param.intergralRadius) {
            this->integralBias += bias;
            if(this->param.maxIntegral) {
                range(this->integralBias, this->param.maxIntegral);
            }
            output += this->param.ki * this->integralBias;
        }
    } else {
        this->integralBias = 0;
    }

    this->lastBias = bias;

    return output;
}

/**
 * @brief 得到输出值
 * @param target 控制量目标值
 * @param current 控制量当前值
 * @param minOutput 最小允许输出值
 * @param maxOutput 最大允许输出值
 * @return double 输出值
 */
double PIDController::getOutput(double target, double current, double minOutput, double maxOutput) {
    double output = this->calculateOutput(target, current);

    if(maxOutput) {
        range(output, minOutput, maxOutput);
    }

    return output;
}

/**
 * @brief 得到输出值
 * @param target 控制量目标值
 * @param current 控制量当前值
 * @param maxOutput 最大允许输出值
 * @return double 输出值
 */
double PIDController::getOutput(double target, double current, double maxOutput) {
    double output = this->calculateOutput(target, current);

    if(maxOutput) {
        range(output, maxOutput);
    }

    return output;
}

/**
 * @brief 达到控制精度
 * @param precision 控制精度(不大于0则使用默认精度)
 * @return true 已经达到控制精度
 * @return false 未达到控制精度
 */
bool PIDController::isFinish(double precision) {
    return std::abs(this->bias) < (precision > 0 ? precision : this->precision);
}

/**
 * @brief 启动控制器
 */
void PIDController::start() {
    this->enable = true;
}

/**
 * @brief 关闭控制器
 */
void PIDController::stop() {
    this->enable = false;
}

/**
 * @brief 计算误差值
 * @param target 控制量目标值
 * @param current 控制量当前值
 * @return double 误差值
 */
double AnglePIDController::calculateBias(double target, double current) {
    return this->bias = utils::angleConvert(current - target);
}
